#ifndef _ASTAR_H
#define _ASTAR_H

#include <iostream>
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <gmm_voxel_map/gmm_voxel_map.h>
#include <queue>
#include "formation_avoidance/GridCube.h"
#include "formation_avoidance/utils.h"

class GridMapNode
{
public:
	enum node_state
	{
		OPENSET = 1,
		CLOSEDSET = 2,
		UNDEFINED = 3
	};
	GridMapNode(){}
	GridMapNode(Eigen::Vector3i node_index):index(node_index),g_score(0),h_score(0),f_score(0),state(UNDEFINED){}
	~GridMapNode(){}

	Eigen::Vector3i index;
	double g_score;
	double h_score;
	double f_score;
	int rounds{0};
	enum node_state state;

    GridMapNode* parent_node;

	// inline bool checkOccupancy()
	// {

	// }

	// bool NodeKnown();
	// double getNodeDistanceInEDTMap();
	// double NodeInformationGain();

	// std::vector<GridMapNode> getbresenham3D(GridMapNode* bresenham3Dend_ptr);
	int tmp;


};
typedef GridMapNode *GridMapNodePtr;


class NodeComparator
{
public:
	bool operator()(GridMapNode* node1, GridMapNode* node2)
	{
		return node1->f_score > node2->f_score;
	}
};

class Astar
{
public:
	Astar(){}
	~Astar(){}
	double getDiagHeu(GridMapNode* node1, GridMapNode* node2);
	inline double getHeu(GridMapNode* node1,GridMapNode* node2){return tie_breaker_ * getDiagHeu(node1, node2);}
	bool a_star_search(Eigen::Vector3d start_p,Eigen::Vector3d end_p,double AstarSafetyDistance,Eigen::Vector3d& local_goal,std::vector<Eigen::Vector3i>& a_star_path );
	void AstarLinkMap(gvm::GMMVoxelMapCircularBuffer* gvmMap,double set_local_length,int set_agent_id);
	void a_star_retrievePath(GridMapNode* final_node);
	void a_star_RemoveCollinearPath();
	double getpointdistance(Eigen::Vector3i idx1, Eigen::Vector3i idx2);
	void a_star_bresenham3D(double bresenham3DSafetyDistance);
	bool corridor_generation(double CorridorGenerationSafetyDistance);
	void initGridMap();

public:

	gvm::GMMVoxelMapCircularBuffer* AstarMapPtr;
	std::priority_queue<GridMapNode*, std::vector<GridMapNode*>, NodeComparator> openSet_;
	std::vector<Eigen::Vector3i> a_star_gridpath;
	const double tie_breaker_ = 1+ 1.0 / 10000;
	int rounds_{0};
	int pool_size{150};
	int pool_size_z{10};
	GridMapNodePtr ***GridMap_;
	double local_length;
	std::vector<GridCube> CorridorCube;
	int agent_id;
	int color_id;
	GridCube cube_inflater;




};



#endif